#include "qelib.h"
#include "qe_sdhci.h"
#include "libfdt.h"
#include "xil_io.h"

QELOG_DOMAIN("psu-emmc");

#define XSDPS_APP_CMD_PREFIX	 0x8000U
#define CMD0	 0x0000U
#define CMD1	 0x0100U
#define CMD2	 0x0200U
#define CMD3	 0x0300U
#define CMD4	 0x0400U
#define CMD5	 0x0500U
#define CMD6	 0x0600U
#define ACMD6	(XSDPS_APP_CMD_PREFIX + 0x0600U)
#define CMD7	 0x0700U
#define CMD8	 0x0800U
#define CMD9	 0x0900U
#define CMD10	 0x0A00U
#define CMD11	 0x0B00U
#define CMD12	 0x0C00U
#define ACMD13	 (XSDPS_APP_CMD_PREFIX + 0x0D00U)
#define CMD16	 0x1000U
#define CMD17	 0x1100U
#define CMD18	 0x1200U
#define CMD19	 0x1300U
#define CMD21	 0x1500U
#define CMD23	 0x1700U
#define ACMD23	 (XSDPS_APP_CMD_PREFIX + 0x1700U)
#define CMD24	 0x1800U
#define CMD25	 0x1900U
#define CMD41	 0x2900U
#define ACMD41	 (XSDPS_APP_CMD_PREFIX + 0x2900U)
#define ACMD42	 (XSDPS_APP_CMD_PREFIX + 0x2A00U)
#define ACMD51	 (XSDPS_APP_CMD_PREFIX + 0x3300U)
#define CMD52	 0x3400U
#define CMD55	 0x3700U
#define CMD58	 0x3A00U

#define XSDPS_CMD_RESP_SEL_MASK		0x00000003U /**< Response Type
							Select */
#define XSDPS_CMD_RESP_NONE_MASK	0x00000000U /**< No Response */
#define XSDPS_CMD_RESP_L136_MASK	0x00000001U /**< Response length 138 */
#define XSDPS_CMD_RESP_L48_MASK		0x00000002U /**< Response length 48 */
#define XSDPS_CMD_RESP_L48_BSY_CHK_MASK	0x00000003U /**< Response length 48 &
                            check busy after
							response */
#define XSDPS_CMD_CRC_CHK_EN_MASK	0x00000008U /**< Command CRC Check
							Enable */
#define XSDPS_CMD_INX_CHK_EN_MASK	0x00000010U /**< Command Index Check
							Enable */
#define RESP_NONE	(u32)XSDPS_CMD_RESP_NONE_MASK
#define RESP_R1		(u32)XSDPS_CMD_RESP_L48_MASK | (u32)XSDPS_CMD_CRC_CHK_EN_MASK | \
			(u32)XSDPS_CMD_INX_CHK_EN_MASK

#define RESP_R1B	(u32)XSDPS_CMD_RESP_L48_BSY_CHK_MASK | \
			(u32)XSDPS_CMD_CRC_CHK_EN_MASK | (u32)XSDPS_CMD_INX_CHK_EN_MASK

#define RESP_R2		(u32)XSDPS_CMD_RESP_L136_MASK | (u32)XSDPS_CMD_CRC_CHK_EN_MASK
#define RESP_R3		(u32)XSDPS_CMD_RESP_L48_MASK

#define RESP_R6		(u32)XSDPS_CMD_RESP_L48_BSY_CHK_MASK | \
			(u32)XSDPS_CMD_CRC_CHK_EN_MASK | (u32)XSDPS_CMD_INX_CHK_EN_MASK

enum {
    MMC_CARD_SD = 1,
    MMC_CARD_MMC,
    MMC_CARD_SDIO,
    MMC_CARD_SDCOMBO,
    MMC_CARD_EMMC,
};

typedef struct 
{
    qe_dev dev;
    qe_u32 iobase;
    qe_u8  version;
    qe_u32 caps;
    qe_u32 clock;
    qe_u16 transfer_mode;
    qe_u32 card_type;
    qe_bool has_cd;
    qe_bool hcs;
    qe_u32 cid[4];
    qe_u32 csd[4];
    qe_u32 rca;
    qe_u32 blklen;
    qe_u32 device_size;
    qe_u32 mult;
    qe_u32 num_sectors;
} psu_sd_host;

static inline void host_writel(psu_sd_host *host, qe_uint reg, qe_u32 val)
{
    // volatile qe_u32 *p = (volatile qe_u32 *)(host->iobase + reg);
    // *p = val;
    Xil_Out32(host->iobase + reg, val);
}

static inline void host_writew(psu_sd_host *host, qe_uint reg, qe_u16 val)
{
    // volatile qe_u16 *p = (volatile qe_u16 *)(host->iobase + reg);
    // *p = val;
    Xil_Out16(host->iobase + reg, val);
}

static inline void host_writeb(psu_sd_host *host, qe_uint reg, qe_u8 val)
{
    // volatile qe_u8 *p = (volatile qe_u8 *)(host->iobase + reg);
    // *p = val;
    Xil_Out8(host->iobase + reg, val);
}

static inline qe_u8 host_readb(psu_sd_host *host, qe_uint reg)
{
    //return *(volatile qe_u8 *)(host->iobase + reg);
    return Xil_In8(host->iobase + reg);
}

static inline qe_u16 host_readw(psu_sd_host *host, qe_uint reg)
{
    //return *(volatile qe_u16 *)(host->iobase + reg);
    return Xil_In16(host->iobase + reg);
}

static inline qe_u32 host_readl(psu_sd_host *host, qe_uint reg)
{
    //return *(volatile qe_u32 *)(host->iobase + reg);
    return Xil_In32(host->iobase + reg);
}

qe_u32 frame_cmd(psu_sd_host *host, qe_u32 cmd)
{
		qe_u32 ret;

		ret = cmd;

		switch(cmd) {
		case CMD0:
			ret |= RESP_NONE;
		break;
		case CMD1:
			ret |= RESP_R3;
		break;
		case CMD2:
			ret |= RESP_R2;
		break;
		case CMD3:
			ret |= RESP_R6;
		break;
		case CMD4:
			ret |= RESP_NONE;
			break;
		case CMD5:
			ret |= RESP_R1B;
		break;
		case CMD6:
			if (host->card_type == MMC_CARD_SD) {
				ret |= RESP_R1 | (qe_u32)SDHCI_DAT_PRESENT_SEL;
			} else {
				ret |= RESP_R1B;
			}
			break;
		case ACMD6:
			ret |= RESP_R1;
		break;
		case CMD7:
			ret |= RESP_R1;
		break;
		case CMD8:
			if (host->card_type == MMC_CARD_SD) {
				ret |= RESP_R1;
			} else {
				ret |= RESP_R1 | (qe_u32)SDHCI_DAT_PRESENT_SEL;
			}
			break;
		case CMD9:
			ret |= RESP_R2;
		break;
		case CMD11:
		case CMD10:
		case CMD12:
        case ACMD13:
		case CMD16:
			ret |= RESP_R1;
		break;
		case CMD17:
		case CMD18:
		case CMD19:
		case CMD21:
			ret |= RESP_R1 | (qe_u32)SDHCI_DAT_PRESENT_SEL;
		break;
		case CMD23:
		case ACMD23:
		case CMD24:
		case CMD25:
			ret |= RESP_R1 | (qe_u32)SDHCI_DAT_PRESENT_SEL;
		break;
		case ACMD41:
			ret |= RESP_R3;
		break;
		case ACMD42:
			ret |= RESP_R1;
		break;
		case ACMD51:
			ret |= RESP_R1 | (qe_u32)SDHCI_DAT_PRESENT_SEL;
		break;
		case CMD52:
		case CMD55:
			ret |= MMC_RSP_R1;
		break;
		case CMD58:
		break;
		default :
			ret |= cmd;
		break;
		}

		return ret;
}

static qe_u32 cmd_transfer(psu_sd_host *host, qe_u32 cmd, qe_u32 arg, qe_u32 blkcnt)
{
    qe_u32 status_reg;
    qe_u32 err_int_sts;
    qe_u32 present_state;
    qe_u32 command_reg;

	/*
	 * Check the command inhibit to make sure no other
	 * command transfer is in progress
	 */
    present_state = host_readl(host, SDHCI_REG_PRESENT_STATE);
    if (present_state & SDHCI_CMD_INHIBIT) {
        qe_debug("cmd inhibit");
        return qe_err_common;
    }
    qe_debug("present_state:%x", present_state);

    /* Write block count register */
    host_writew(host, SDHCI_REG_BLOCK_COUNT, (qe_u16)blkcnt);

    host_writeb(host, SDHCI_REG_TIMEOUT_CTRL, 0xe);

    /* Write argument register */
    host_writel(host, SDHCI_REG_ARGUMENT, arg);

    host_writew(host, SDHCI_REG_NOR_INT_STS, 0xFFFF);
    host_writew(host, SDHCI_REG_ERR_INT_STS, 0xF3FF);

    /* Command register is set to trigger transfer of command */
    command_reg = frame_cmd(host, cmd);
    qe_debug("command_reg:%x", command_reg);
	/*
	 * Mask to avoid writing to reserved bits 31-30
	 * This is necessary because 0x80000000 is used  by this software to
	 * distinguish between ACMD and CMD of same number
	 */
    command_reg = command_reg & 0x3FFFU;

	/*
	 * Check for data inhibit in case of command using DAT lines.
	 * For Tuning Commands DAT lines check can be ignored.
	 */
    if ((cmd != CMD21) && (cmd != CMD19)) {
        present_state = host_readl(host, SDHCI_REG_PRESENT_STATE);
        if ((present_state & (SDHCI_DATA_INHIBIT | SDHCI_CMD_INHIBIT)) &&
            (command_reg & SDHCI_CMD_DATA))
        return qe_err_common;
    }

    host_writel(host, SDHCI_REG_TRANSFER_MODE, (command_reg << 16) | host->transfer_mode);
    qe_debug("xfer mode:%x", host_readl(host, SDHCI_REG_TRANSFER_MODE));

    /* Polling for response for now */
    do {
        status_reg = host_readw(host, SDHCI_REG_NOR_INT_STS);
        if ((cmd == CMD21) || (cmd == CMD19)) {
            if (status_reg & SDHCI_INT_DATA_AVAIL) {
                host_writel(host, SDHCI_REG_NOR_INT_STS, SDHCI_INT_DATA_AVAIL);
                break;
            }
        }

        if (status_reg & 0x8000) {
            err_int_sts = host_readw(host, SDHCI_REG_ERR_INT_STS);
            
            if (!(err_int_sts & ~0x01))
                err_int_sts = 0x02;
            
            /* Write to clear error bits */
            host_writew(host, SDHCI_REG_ERR_INT_STS, SDHCI_ERR_INT_ALL_MASK);
        }
    } while((status_reg & SDHCI_INT_RESPONSE) == 0U);

    /* Write to clear bit */
    host_writew(host, SDHCI_REG_NOR_INT_STS, SDHCI_INT_RESPONSE);

    return qe_ok;
}

static qe_u32 change_clkfreq(psu_sd_host *host, qe_u32 freq)
{
   qe_u16 cnt;
   qe_u16 div;
   qe_u16 ext_div = 0;
   qe_u16 clkreg;
   qe_u32 ret;

   clkreg = host_readl(host, SDHCI_REG_CLOCK_CTRL);
   clkreg &= ~(SDHCI_CLOCK_CARD_EN | SDHCI_CLOCK_INT_STABLE);
   host_writel(host, SDHCI_REG_CLOCK_CTRL, clkreg);
   qe_debug("disable clk");

   if (host->version == SDHCI_SPEC_300) {

       /* Calculate divisor */
		for (cnt=1; cnt<=SDHCI_CC_EXT_MAX_DIV_CNT; cnt++) {
			if (((host->clock) / cnt) <= freq) {
				div = cnt >> 1;
				break;
			}
		}

		if (cnt > SDHCI_CC_EXT_MAX_DIV_CNT) {
			/* No valid divisor found for given frequency */
			ret = qe_err_common;
			goto __exit;
		}
   } else {

       cnt = 1;
       while (cnt <= SDHCI_CC_MAX_DIV_CNT) {
			if (((host->clock) / cnt) <= freq) {
				div = cnt / 2U;
				break;
			}
			cnt = cnt << 1U;
       }

		if (cnt > SDHCI_CC_MAX_DIV_CNT) {
			/* No valid divisor found for given frequency */
			ret = qe_err_common;
			goto __exit;
		}
   }

   /* Set clock divisor */
	if (host->version == SDHCI_SPEC_300) {
		clkreg = host_readw(host, SDHCI_REG_CLOCK_CTRL);
		clkreg &= ~(SDHCI_CC_SDCLK_FREQ_SEL_MASK | SDHCI_CC_SDCLK_FREQ_SEL_EXT_MASK);

		ext_div = div >> 8;
		ext_div <<= SDHCI_DIVIDER_HI_SHIFT;
		ext_div &= SDHCI_CC_SDCLK_FREQ_SEL_EXT_MASK;

		div <<= SDHCI_DIVIDER_SHIFT;
		div &= SDHCI_CC_SDCLK_FREQ_SEL_MASK;
		clkreg |= div | ext_div | (qe_u16)SDHCI_CLOCK_INT_EN;
		host_writew(host, SDHCI_REG_CLOCK_CTRL, clkreg);
	} else {
		clkreg = host_readw(host, SDHCI_REG_CLOCK_CTRL);
		clkreg &= (~SDHCI_CC_SDCLK_FREQ_SEL_MASK);

		div <<= SDHCI_DIVIDER_SHIFT;
		div &= SDHCI_CC_SDCLK_FREQ_SEL_MASK;
		clkreg |= div | (qe_u16)SDHCI_CLOCK_INT_EN;
		host_writew(host, SDHCI_REG_CLOCK_CTRL, clkreg);
	}

	/* Wait for internal clock to stabilize */
	while(1) {
		if (host_readw(host, SDHCI_REG_CLOCK_CTRL) & 0x02)
           break;
	}

    qe_debug("clock stable");

	/* Enable SD clock */
	clkreg = host_readw(host, SDHCI_REG_CLOCK_CTRL);
	host_writew(host, SDHCI_REG_CLOCK_CTRL, clkreg | SDHCI_CLOCK_CARD_EN);

   ret = qe_ok;

__exit:
   return ret;
}

static void host_init(psu_sd_host *host)
{
    qe_ret ret;
    qe_u8 power_level;
    qe_u32 read_reg;

    qe_debug("host init");

    if ((host_readw(host, 0xFE) & 0xFF) == 0x02) {
        qe_debug("spec v3, set emmc hw reset");
        host_writeb(host, SDHCI_REG_POWER_CTRL, SDHCI_PC_VDD2_ON);
    } else {
        host_writeb(host, SDHCI_REG_POWER_CTRL, 0);
    }

    qe_usleep(1000);

    host_writeb(host, SDHCI_REG_RESET, SDHCI_RESET_ALL);

    read_reg = host_readb(host, SDHCI_REG_RESET);
    while ((read_reg & SDHCI_RESET_ALL) != 0) {
        read_reg = host_readb(host, SDHCI_REG_RESET);
    }

    qe_debug("host reset done");

    host->version = (qe_u8)(host_readw(host, SDHCI_REG_VERSION) & 0xFF);

    host->caps = host_readl(host, SDHCI_REG_CAPABILITIES);
    qe_debug("read caps:0x%x", host->caps);

    /* Select voltage and enable bus power. */
    if (host->version == SDHCI_SPEC_300) {
        host_writeb(host, SDHCI_REG_POWER_CTRL, (SDHCI_PC_VDD1_3V3 | SDHCI_PC_VDD1_ON) & ~SDHCI_PC_VDD2_ON);
    } else {
        host_writeb(host, SDHCI_REG_POWER_CTRL, SDHCI_PC_VDD1_3V3 | SDHCI_PC_VDD1_ON);
    }

    /* Delay before issuing the command after emmc reset */
    if (host->version == SDHCI_SPEC_300) {
        if ((host->caps & SDHCI_CAP_SLOT_TYPE) == SDHCI_CAP_EMB_SLOT) {
            qe_debug("delay 200us");
            qe_usleep(200);
        }
    }

    /* Change the clock frequency to 400 KHz */
    ret = change_clkfreq(host, SDHCI_CLK_400_KHZ);
    if (ret != qe_ok) {
        qe_error("cheng clock to 400Khz err:%d", ret);
        return;
    }
    qe_debug("clock change to 400khz");
    
    if ((host->caps & SDHCI_CAP_VDD_330) != 0U) {
		power_level = SDHCI_PC_VDD1_3V3;
	} else if ((host->caps & SDHCI_CAP_VDD_300) != 0U) {
		power_level = SDHCI_PC_VDD1_3V0;
	} else if ((host->caps & SDHCI_CAP_VDD_180) != 0U) {
		power_level = SDHCI_PC_VDD1_1V8;
	} else {
		power_level = 0U;
	}

	/* Select voltage based on capability and enable bus power. */
	host_writeb(host, SDHCI_REG_POWER_CTRL, power_level | SDHCI_PC_VDD1_ON);
    qe_debug("enable bus power %x", power_level);

	if (host->version == SDHCI_SPEC_300) {
		/* Enable ADMA2 in 64bit mode. */
		host_writeb(host, SDHCI_REG_HOST_CTRL, SDHCI_CTRL_ADMA64);
        qe_debug("enable AMDA2 64bit mode");
	} else {
		/* Enable ADMA2 in 32bit mode. */
        host_writeb(host, SDHCI_REG_HOST_CTRL, SDHCI_CTRL_ADMA32);
	}

	/* Enable all interrupt status except card interrupt initially */
	host_writew(host, SDHCI_REG_NOR_INT_ENABLE, 0xFFFF & (~SDHCI_INT_CARD_INT));
    qe_debug("enable all interrupt");

	host_writew(host, SDHCI_REG_ERR_INT_ENABLE, 0xF3FF);

    host_writew(host, SDHCI_REG_NOR_SIG_ENABLE, 0x0);
    host_writew(host, SDHCI_REG_ERR_SIG_ENABLE, 0x0);

    host->transfer_mode = SDHCI_TRNS_DMA | SDHCI_TRNS_BLK_CNT_EN | SDHCI_TRNS_READ;

    /* Set block size to 512 by default */
    host_writew(host, SDHCI_REG_BLOCK_SIZE, 0x200);

    qe_debug("host init done");
}

static qe_ret mmc_card_init(psu_sd_host *host)
{
	qe_ret ret;
    qe_u32 ocr;
    qe_u32 present_state;

    qe_debug("mmc card init");

    if ((host->version != SDHCI_SPEC_300) || 
        ((host->caps & SDHCI_CAP_SLOT_TYPE) != SDHCI_CAP_EMB_SLOT)) {
        if (host->has_cd) {
    		/*
			 * Check the present state register to make sure
			 * card is inserted and detected by host controller
			 */
            present_state = host_readl(host, SDHCI_REG_PRESENT_STATE);
            if ((present_state & SDHCI_PS_CARD_INSRT) == 0) {
                qe_error("cd not detect");
                return qe_err_notfind;
            }
        }
    }

    /* CMD0 no response expected */
    ret = cmd_transfer(host, MMC_CMD_GO_IDLE_STATE, 0, 0);
    if (ret != qe_ok) {
        return ret;
    }

    qe_debug("CMD0");

    ocr = 0;
    /* Send CMD1 while card is still busy with power up */
    while ((ocr & 0x80000000U) == 0) {
        /* Host High Capacity support & High volage window */
        ret = cmd_transfer(host, CMD1, 0x40000000U | 0x00FF8000U, 0);
        if (ret != qe_ok)
            return ret;

        /* Response with card capacity */
        ocr = host_readl(host, SDHCI_REG_RESP0);
    }

    qe_debug("wait busy done OCR %x", ocr);

    if (ocr & MMC_OCR_HCS)
        host->hcs = 1;

    /* CMD2 for Card ID */
    ret = cmd_transfer(host, CMD2, 0, 0);
    if (ret != qe_ok) {
        qe_error("CMD2 err:%d", ret);
        return ret;
    }

    qe_debug("CMD2");

    host->cid[0] = host_readw(host, SDHCI_REG_RESP0);
    host->cid[1] = host_readw(host, SDHCI_REG_RESP1);
    host->cid[2] = host_readw(host, SDHCI_REG_RESP2);
    host->cid[3] = host_readw(host, SDHCI_REG_RESP3);
    qe_info("cid: %x %x %x %x", host->cid[0], host->cid[1], host->cid[2], host->cid[3]);

    /* Set relative card address */
    host->rca = 0x12340000;
    ret = cmd_transfer(host, CMD3, host->rca, 0);
    if (ret != qe_ok) {
        qe_error("CMD3 err:%d", ret);
        return ret;
    }

    ret = cmd_transfer(host, CMD9, host->rca, 0);
    if (ret != qe_ok) {
        qe_error("CMD3 err:%d", ret);
        return ret;
    }

    /*
	 * Card specific data is read.
	 * Currently not used for any operation.
	 */
    host->csd[0] = host_readl(host, SDHCI_REG_RESP0);
    host->csd[1] = host_readl(host, SDHCI_REG_RESP1);
    host->csd[2] = host_readl(host, SDHCI_REG_RESP2);
    host->csd[3] = host_readl(host, SDHCI_REG_RESP3);
    qe_info("csd: %x %x %x %x", host->csd[0], host->csd[1], host->csd[2], host->csd[3]);


	/* Calculating the memory capacity */
	host->blklen = 1 << ((host->csd[2] & 0xf00) >> 8U);
	host->mult = 1 << (((host->csd[1] & 0x380) >> 7U) + 2U);
	host->device_size = (host->csd[1] & 0xffc00000) >> 22U;
	host->device_size |= (host->csd[2] & 0x00000003) << 10U;
	host->device_size = (host->device_size + 1U) * host->mult;
	host->device_size =  host->device_size * host->blklen;

    host->num_sectors = host->device_size / 512;

    qe_info("blklen:%d mult:%d size:%d sectors:%d", host->blklen, host->mult, host->device_size, host->num_sectors);

    return qe_ok;
}

static qe_ret psu_sdhci_probe(const void *fdt, int offset)
{
    int len;
    const char *name;
    const qe_u32 *prop_reg;
    psu_sd_host *host;

    qelog_domain_set_level("psu-emmc", QELOG_DEBUG);

    qe_debug("psu sdhci probe");

    name = fdt_get_name(fdt, offset, QE_NULL);

    prop_reg = (const qe_u32 *)fdt_getprop(fdt, offset, "reg", &len);
    if (!prop_reg) {
        qe_error("%s no reg prop", name);
        return qe_err_param;
    }

    host = qe_malloc(sizeof(psu_sd_host));
    qe_assert(host);
    qe_memset(host, 0, sizeof(psu_sd_host));

    host->iobase = (qe_u32)fdt32_to_cpu(*prop_reg);
    host->clock = 187498123;

    qe_debug("iobase %x", host->iobase);

    host_init(host);

    mmc_card_init(host);

    return qe_ok;
}

// static const qe_device_id psu_sdhci_ids[] = {
//     {.compatible="xlnx,zynqmp-sdhci"},
//     {}
// };

// QE_DRIVER(zynqmp_sdhci) = {
//     .name = "zynqmp-sdhci",
//     .of_match = psu_sdhci_ids,
//     .probe = psu_sdhci_probe,
// };
